package com.dizner.androidcartest;

import android.os.Handler;
import android.os.SystemClock;
import android.util.Log;

import com.google.android.things.pio.Gpio;
import com.google.android.things.pio.PeripheralManager;
import com.google.android.things.pio.Pwm;

import java.io.IOException;
import java.util.HashMap;
import java.util.Iterator;
import java.util.List;
import java.util.Map;
import java.util.concurrent.ConcurrentHashMap;

public class L298NDiver implements AutoCloseable {

    private static final String TAG = "L298NDiver";

    private String M1_ENABLE_GPIO_NAME = "";
    private String M2_ENABLE_GPIO_NAME = "";
    private String M1_A_GPIO_NAME = "";
    private String M1_B_GPIO_NAME = "";
    private String M2_A_GPIO_NAME = "";
    private String M2_B_GPIO_NAME = "";

    private Gpio M1_A, M1_B, M2_A, M2_B;

    private Pwm M1_EN, M2_EN;
    private boolean m1Enable = true, m2Enable = true;
    private double FrequencyHz = 100;
    private double DefaultDutyCycle = 100;
    private double tempDutyCycle = 0;

    private Map<String, Long> forwardMsgList = new ConcurrentHashMap<>();//前进待处理指令集
    private Map<String, Long> backMsgList = new ConcurrentHashMap<>();//后退待处理指令集
    private Map<String, Long> rightMsgList = new ConcurrentHashMap<>();//左转待处理指令集
    private Map<String, Long> leftMsgList = new ConcurrentHashMap<>();//右转待处理指令集
    private Map<String, Long> stopMsgList = new ConcurrentHashMap<>();//刹车待处理指令集

    private boolean enableForward = true, enableLeft = true, enableRight = true, enableBack = true;


    private L298NDiver(String m1EnableGpioName, String m2EnableGpioName, String m1aGpioName, String m1bGpioName, String m2aGpioName, String m2bGpioName) {
        this.M1_ENABLE_GPIO_NAME = m1EnableGpioName;
        this.M2_ENABLE_GPIO_NAME = m2EnableGpioName;
        this.M1_A_GPIO_NAME = m1bGpioName;
        this.M1_B_GPIO_NAME = m2aGpioName;
        this.M2_A_GPIO_NAME = m1aGpioName;
        this.M2_B_GPIO_NAME = m2bGpioName;
        config(this);
    }

    static L298NDiver create(String m1EnableGpioName, String m2EnableGpioName, String m1aGpioName, String m1bGpioName, String m2aGpioName, String m2bGpioName) {


        return new L298NDiver(m1EnableGpioName, m2EnableGpioName, m1aGpioName, m1bGpioName, m2aGpioName, m2bGpioName);
    }

    private void config(L298NDiver diver) {
        PeripheralManager pm = PeripheralManager.getInstance();
//        List<String> pwmList = pm.getPwmList();
//        for (String s : pwmList) {
//            Log.i(TAG, s);
//        }
        try {

//            pwmA = pm.openPwm("PWM1");

            M1_EN = pm.openPwm(M1_ENABLE_GPIO_NAME);
            M2_EN = pm.openPwm(M2_ENABLE_GPIO_NAME);
            M1_A = pm.openGpio(M1_A_GPIO_NAME);
            M1_B = pm.openGpio(M1_B_GPIO_NAME);
            M2_A = pm.openGpio(M2_A_GPIO_NAME);
            M2_B = pm.openGpio(M2_B_GPIO_NAME);

            //刹车
//            M1_EN.setDirection(Gpio.DIRECTION_OUT_INITIALLY_LOW);
            M1_A.setDirection(Gpio.DIRECTION_OUT_INITIALLY_LOW);
            M1_B.setDirection(Gpio.DIRECTION_OUT_INITIALLY_LOW);


            //刹车
//            M2_EN.setDirection(Gpio.DIRECTION_OUT_INITIALLY_LOW);
            M2_A.setDirection(Gpio.DIRECTION_OUT_INITIALLY_LOW);
            M2_B.setDirection(Gpio.DIRECTION_OUT_INITIALLY_LOW);


            try {
                M1_EN.setPwmFrequencyHz(FrequencyHz);
                M1_EN.setPwmDutyCycle(DefaultDutyCycle);
                M2_EN.setPwmFrequencyHz(FrequencyHz);
                M2_EN.setPwmDutyCycle(DefaultDutyCycle);
                tempDutyCycle = DefaultDutyCycle;
                M1_EN.setEnabled(true);
                M2_EN.setEnabled(true);
            } catch (IOException e) {
                e.printStackTrace();
            }

            ready();

        } catch (IOException e) {
            e.printStackTrace();
        }
    }


    private Thread forwardThread = new Thread() {
        @Override
        public void run() {
            while (true) {
                if (forwardMsgList != null) {
                    Iterator<Map.Entry<String, Long>> iterator = forwardMsgList.entrySet().iterator();
                    while (iterator.hasNext()) {
                        Map.Entry<String, Long> entry = iterator.next();
                        if (entry.getValue() < 0) {
                            continue;
                        }
                        try {
                            L298NDiver.this.stop();
                            M1_A.setValue(true);
                            M1_B.setValue(false);

                            M2_A.setValue(false);
                            M2_B.setValue(true);
                        } catch (IOException e) {
                            e.printStackTrace();
                        }
                        delayedAndStop(entry.getValue());
                        iterator.remove();
                    }
                }
            }
        }
    };


    private Thread backThread = new Thread() {
        @Override
        public void run() {
            while (true) {
                if (backMsgList != null) {
                    Iterator<Map.Entry<String, Long>> iterator = backMsgList.entrySet().iterator();
                    while (iterator.hasNext()) {
                        Map.Entry<String, Long> entry = iterator.next();
                        if (entry.getValue() < 0) {
                            continue;
                        }
                        try {
                            L298NDiver.this.stop();
                            M1_A.setValue(false);
                            M1_B.setValue(true);

                            M2_A.setValue(true);
                            M2_B.setValue(false);
                        } catch (IOException e) {
                            e.printStackTrace();
                        }
                        delayedAndStop(entry.getValue());
                        iterator.remove();
                    }
                }
            }
        }
    };


    private Thread rightThread = new Thread() {
        @Override
        public void run() {
            while (true) {
                if (rightMsgList != null) {
                    Iterator<Map.Entry<String, Long>> iterator = rightMsgList.entrySet().iterator();
                    while (iterator.hasNext()) {
                        Map.Entry<String, Long> entry = iterator.next();
                        if (entry.getValue() < 0) {
                            continue;
                        }

                        try {
                            L298NDiver.this.stop();
                            if (m1Enable) {
                                setMEnabled(M1_EN, false);
                                M1_A.setValue(true);
                                M1_B.setValue(false);
                            }
                            if (m2Enable) {
                                setMEnabled(M2_EN, true);
                                M2_A.setValue(false);
                                M2_B.setValue(true);
                            }
                        } catch (IOException e) {
                            e.printStackTrace();
                        }

                        delayedAndStop(entry.getValue());

                        if (m1Enable) {
                            setMEnabled(M1_EN, true);
                        }
                        if (m2Enable) {
                            setMEnabled(M2_EN, true);
                        }
                        iterator.remove();
                    }
                }
            }
        }
    };


    private Thread leftThread = new Thread() {
        @Override
        public void run() {
            while (true) {
                if (leftMsgList != null) {
                    Iterator<Map.Entry<String, Long>> iterator = leftMsgList.entrySet().iterator();
                    while (iterator.hasNext()) {
                        Map.Entry<String, Long> entry = iterator.next();
                        if (entry.getValue() < 0) {
                            continue;
                        }

                        try {
                            L298NDiver.this.stop();
                            if (m1Enable) {
                                setMEnabled(M1_EN, true);
                                M1_A.setValue(true);
                                M1_B.setValue(false);
                            }

                            if (m2Enable) {
                                setMEnabled(M2_EN, false);
                                M2_A.setValue(false);
                                M2_B.setValue(true);
                            }
                        } catch (IOException e) {
                            e.printStackTrace();
                        }


                        delayedAndStop(entry.getValue());

                        if (m1Enable) {
                            setMEnabled(M1_EN, true);
                        }
                        if (m2Enable) {
                            setMEnabled(M2_EN, true);
                        }
                        iterator.remove();
                    }
                }
            }
        }
    };


    private Thread stopThread = new Thread() {
        @Override
        public void run() {
            while (true) {
                if (stopMsgList != null) {
                    Iterator<Map.Entry<String, Long>> iterator = stopMsgList.entrySet().iterator();
                    while (iterator.hasNext()) {
                        Map.Entry<String, Long> entry = iterator.next();
                        if (entry.getValue() < 0) {
                            continue;
                        }

                        //刹车
                        try {
                            M1_A.setValue(false);
                            M1_B.setValue(false);
                            //刹车
                            M2_A.setValue(false);
                            M2_B.setValue(false);
                        } catch (IOException e) {
                            e.printStackTrace();
                        }
                        delayedAndStop(entry.getValue());

                        iterator.remove();
                    }
                }
            }
        }
    };

    void forward(long miles) {
        if (enableForward) {
            forwardMsgList.put(SystemClock.currentThreadTimeMillis() + "", miles);
        }

        if (!forwardThread.isAlive()) {
            forwardThread.start();
        }
    }

    public void forwardEnd() {
        if (forwardMsgList != null) {
            forwardMsgList.clear();
        }
    }


    void back(long miles) {
        if (enableBack) {
            backMsgList.put(SystemClock.currentThreadTimeMillis() + "", miles);
        }


        if (!backThread.isAlive()) {
            backThread.start();
        }

    }


    public void backEnd() {
        if (backMsgList != null) {
            backMsgList.clear();
        }
    }


    void right(long miles) {
        if (enableRight) {
            rightMsgList.put(SystemClock.currentThreadTimeMillis() + "", miles);
        }


        if (!rightThread.isAlive()) {
            rightThread.start();
        }
    }

    public void rightEnd() {
        if (rightMsgList != null) {
            rightMsgList.clear();
        }
    }


    void left(long miles) {
        if (enableLeft) {
            leftMsgList.put(SystemClock.currentThreadTimeMillis() + "", miles);
        }


        if (!leftThread.isAlive()) {
            leftThread.start();
        }
    }


    public void leftEnd() {
        if (leftMsgList != null) {
            leftMsgList.clear();
        }
    }

    void ready() {

        try {
            stop();
            setMEnabled(M1_EN, true);
            setMEnabled(M2_EN, true);
            m1Enable = true;
            m2Enable = true;

            M1_A.setValue(false);
            M1_B.setValue(false);
            //刹车
            M2_A.setValue(false);
            M2_B.setValue(false);
        } catch (IOException e) {
            e.printStackTrace();
        }

    }

    private void setMEnabled(Pwm pwm, boolean enable) {
        try {
            pwm.setEnabled(enable);
            pwm.setPwmFrequencyHz(FrequencyHz);
            pwm.setPwmDutyCycle(tempDutyCycle);
        } catch (IOException e) {
            e.printStackTrace();
        }
    }


    void stop() {
        //刹车
        try {
            M1_A.setValue(false);
            M1_B.setValue(false);
            //刹车
            M2_A.setValue(false);
            M2_B.setValue(false);
        } catch (IOException e) {
            e.printStackTrace();
        }
    }

    //刹车
    void stop(long miles) {
        stopMsgList.put(SystemClock.currentThreadTimeMillis() + "", miles);

        if (!stopThread.isAlive()) {
            stopThread.start();
        }
    }


    void stopEnd() {
        if (stopMsgList != null) {
            stopMsgList.clear();
        }
    }


    private void delayedAndStop(long miles) {
        if (miles <= 0) {
            return;
        }
        try {
            int temp = 0;

            while (true) {
                temp++;
                Thread.sleep(1);
                if (temp > miles) {
                    break;
                }
            }

        } catch (InterruptedException e) {
            e.printStackTrace();
        }
        stop();
    }


    @Override
    public void close() throws Exception {
        if (M1_A != null) {
            M1_A.close();
        }
        if (M1_B != null) {
            M1_B.close();
        }
        if (M2_A != null) {
            M2_A.close();
        }
        if (M2_B != null) {
            M2_B.close();
        }
    }

    void closed() {
        //刹车
        try {
            M1_EN.setEnabled(false);
            m1Enable = false;
            //刹车
            M2_EN.setEnabled(false);
            m2Enable = false;
        } catch (IOException e) {
            e.printStackTrace();
        }
    }

    public boolean isEnableForward() {
        return enableForward;
    }

    public void setEnableForward(boolean enableForward) {
        this.enableForward = enableForward;
    }

    public boolean isEnableLeft() {
        return enableLeft;
    }

    public void setEnableLeft(boolean enableLeft) {
        this.enableLeft = enableLeft;
    }

    public boolean isEnableRight() {
        return enableRight;
    }

    public void setEnableRight(boolean enableRight) {
        this.enableRight = enableRight;
    }

    public boolean isEnableBack() {
        return enableBack;
    }

    public void setEnableBack(boolean enableBack) {
        this.enableBack = enableBack;
    }

    /**
     * 获取M1使能状态
     *
     * @return
     */
    private boolean getM1EnStatus() {
        return m1Enable;
    }

    /**
     * 获取M2使能状态
     *
     * @return
     */
    private boolean getM2EnStatus() {
        return m2Enable;
    }

    /**
     * 获取使能状态
     *
     * @return
     */
    boolean getEnStatus() {

        return getM1EnStatus() && getM2EnStatus();
    }


    public int getDutyCycle(int maxSpeed) {
        double temp = (tempDutyCycle - 30) * maxSpeed / 70f;

        Log.i(TAG, "当前速度：" + temp);
        return (int) temp;
    }

    /**
     * max 100
     * min 0
     *
     * @param speed
     */
    public void changeSpeed(int maxSpeed, int speed) {
        if (speed > 100 || speed < 0) {
            throw new IllegalArgumentException("Speed ranges from 0 to 100!");
        }
//        if (speed < 1) {
//            speed = 1;
//        }

        float temp = 30 + speed * (70f / maxSpeed);

        if (temp > 100) {
            temp = 100;
        }
        try {
            if (temp <= 30) {
                temp = 0;
            }
            tempDutyCycle = temp;
            M1_EN.setPwmFrequencyHz(FrequencyHz);
            M2_EN.setPwmFrequencyHz(FrequencyHz);
            M1_EN.setPwmDutyCycle(tempDutyCycle);
            M2_EN.setPwmDutyCycle(tempDutyCycle);
        } catch (IOException e) {
            e.printStackTrace();
        }
//        M1_EN.
    }

}
